/*
 * @file peripheral_event.h
 * @author haibo
 * @date Dec 26, 2023
 * @brief Peripheral Event
 * Contact: haibo.yang@lgmgim.cn
 */

#pragma once
#include <agv_msgs/AGVEvent.h>

class CEventManager {
public:
    CEventManager(): seq_(0) {}

    void init(ros::NodeHandle nh) {
        event_pub_ = nh.advertise<agv_msgs::AGVEvent>("/agvevent", 10);
        timer_ = nh.createTimer(ros::Duration(1.0f), boost::bind(&CEventManager::EventPublish, this, _1)); 
    }

    void addEvent(const int ecode, int16_t event_React,int16_t typ, const std::string& message, int param1, int param2) {
        agv_msgs::AGVEvent  event;
        ros::Time timestamp = ros::Time::now();
        event.header.stamp = timestamp;
        event.header.seq = seq_++;
        event.header.frame_id = ros::this_node::getName();
            
        event.ecode = ecode;
        event.description = message;
        event.action = event_React;
        event.type = typ; 
        event.ackReq = false;
        event.param1 = param1;
        event.param2 = param2;
        event.eventTime = timestamp.toSec();
        events_.push_back(event);
        publishEvent(event);
        ROS_INFO_STREAM("[Peripheral] Add and publish event:" << event);
    }

    void updateEvent(const int ecode, int16_t event_React,int16_t typ, const std::string& message, int param1 = 0, int param2 = 0) {
        auto it = std::find_if(events_.begin(), events_.end(), [ecode](const agv_msgs::AGVEvent& event) { 
            return event.ecode == ecode; });
    
        // 判断是否找到事件
        if (it != events_.end()) {
            ros::Time timestamp = ros::Time::now();
            it->header.stamp = timestamp;
            it->header.seq = seq_++;
            it->header.frame_id = ros::this_node::getName();
            it->description = message;
            if (param1 != 0)
                it->param1 = param1;
            if (param2 != 0)
                it->param2 = param2;
            publishEvent(*it);
            ROS_INFO_STREAM("[Peripheral] Update and publish event:" << *it);
        } else {
            addEvent(ecode, event_React, typ,  message, param1, param2);
        }
    }

    void clearEvent() {
        if (events_.size() > 0) {
            events_.clear();
        }
    }

    void removeEvent(const int ecode) {
        if (events_.size() > 0) {
            events_.erase(std::remove_if(events_.begin(), events_.end(), [ecode](const agv_msgs::AGVEvent& event) {
                if (event.ecode == ecode) {
                    ROS_INFO_STREAM("[Peripheral] Delete event:" << event);
                    return true; 
                } else
                    return false; 
                }
            ), events_.end());
        }
    }

    bool IsNomal(void) {
        bool ret= events_.size() == 0;
        return ret;
    }

    bool checkEvent(const int ecode) {
        auto it = std::find_if(events_.begin(), events_.end(), [ecode](const agv_msgs::AGVEvent& event) { 
            return event.ecode == ecode; });

        if (it != events_.end()) {
            return true;
        } else {
            return false;
        }
    }

    void publishEvent(const agv_msgs::AGVEvent& event) {
        event_pub_.publish(event);
    }

    void publishNormal(void) {
        agv_msgs::AGVEvent  event;
        ros::Time timestamp = ros::Time::now();
        event.header.stamp = timestamp;
        event.header.seq = seq_++;
        event.header.frame_id = ros::this_node::getName();
            
        event.ecode = agv_msgs::AGVEvent::NONE_EVENT;
        event.description = "No event";
        event.action = agv_msgs::AGVEvent::NONE;
        event.type = agv_msgs::AGVEvent::INFO;  
        event_pub_.publish(event);  
        // ROS_INFO_STREAM("[Peripheral] Publish normal");
    }

    void EventPublish(const ros::TimerEvent& event) {
        // ROS_INFO_STREAM("[Peripheral] event size:" << events_.size());
        if (!(events_.size() > 0)) {
            publishNormal();
        }
        
    }
private:
    ros::Publisher event_pub_;
    ros::Timer timer_;
    std::vector<agv_msgs::AGVEvent> events_;
    unsigned int seq_;
};